/*
 *  CollisionDetection.cpp
 *  c_Render
 *
 *  Created by liao peterson on 2/22/10.
 *  Copyright 2010 uestc. All rights reserved.
 *
 */
#include"Physics.h"

/**
 *	Spere to sphere contact detector
 **/
bool collisionManager::Sphere2Sphere(physicsIntegrator&one,
									 physicsIntegrator&two,
									 collisionData*data) 
{
	// Cache the sphere positions.
	vec3 positionOne = one.pi.loc;
	vec3 positionTwo = two.pi.loc;
	
	// Find the vector between the objects.
	vec3 midline = positionTwo - positionOne;
	GLfloat size = midline.norm();
	
	// See if it is large enough.
	//GLboolean a = (size >= (one.pi.bRadius + two.pi.bRadius));
	if (size >= (one.pi.bRadius + two.pi.bRadius))
	{
		return false;
	}
	
	// Manually create the normal, because we have the size
	// to hand.
	vec3 normal = midline/size;
	
	contact* temp = new contact;
	normal.normalize();
	temp->contactNormal = normal;
	temp->contactPoint = positionOne + midline/2;
	temp->penetration = (one.pi.bRadius + two.pi.bRadius - size);
	
	temp->one = &one;
	temp->two = &two;
	
	data->contacts->Append(temp);
	
	printf("Sphere to sphere contact detected.\n");
	return true;
}

/**
 *	Box to box contact detector
 **/
inline GLfloat transformToAxis(const physicsIntegrator &p, const vec3 &axis)
{
	box* bb = p.pi.bBox;
	return 		bb->halfSize.x * fabs(axis * p.getAxis(0)) +
	bb->halfSize.y * fabs(axis * p.getAxis(1)) + 
	bb->halfSize.z * fabs(axis * p.getAxis(2));
}

GLfloat overlapOnAxis(const physicsIntegrator&one,
					  const physicsIntegrator&two,
					  const vec3&axis)
{
	// Project the half-size of one ont axis.
	GLfloat oneProject = transformToAxis(one, axis);
	GLfloat twoProject = transformToAxis(two, axis);
	
	// Find the vector between the two centers.
	vec3 toCenter = two.pi.loc - one.pi.loc;
	
	// Project this onto the axis.
	GLfloat distance = fabs(toCenter * axis);
	
	// Check for overlap.
	// >0 for non overlap, <0 for overlap
	return distance - oneProject - twoProject;
}	

GLfloat tryAxis(physicsIntegrator&one,physicsIntegrator&two,
				vec3 axis, GLint index, GLfloat&pen, GLint&best)
{
	if(axis.norm()<EPSILON)return BIG_VALUE;
	axis.normalize();
	GLfloat temp = overlapOnAxis(one, two, axis);
	if(temp > pen){
		pen = temp;
		best = index;
	}
	return temp;
}

// Relaxed version of try axis for preferring edge to edge to point to face
GLfloat rtryAxis(physicsIntegrator&one,physicsIntegrator&two,
				 vec3 axis, GLint index, GLfloat&pen, GLint&best)
{
	if(axis.norm()<EPSILON)return BIG_VALUE;
	axis.normalize();
	GLfloat temp = overlapOnAxis(one, two, axis);
	if(temp + EPSILON <0)
		temp += EPSILON;
	if(temp > pen){
		pen = temp;
		best = index;
	}
	return temp;
}

GLint face2face(physicsIntegrator&one,physicsIntegrator&two,
				GLint bestAxis, GLfloat pen, collisionData*data)
{
	contact * pContact= new contact;
	vec3 axis;
	vec3 toCenter = two.pi.loc - one.pi.loc;
	if(bestAxis<4){
		axis = one.getAxis(bestAxis-1);
		for(GLint i=0;i<3;i++)
			if((axis % two.getAxis(i)).norm() < 0.1){
				axis *= (axis * toCenter > 0)?1:-1;
				pContact->contactNormal = axis;
				pContact->penetration = pen;
				pContact->one = &one;
				pContact->two = &two;
				data->contacts->Append(pContact);
				return 1;
			}
	}else{
		axis = two.getAxis(bestAxis - 4);
		for(GLint i=0;i<3;i++)
			if((axis % one.getAxis(i)).norm() < 0.1){
				axis *= (axis * toCenter > 0)?1:-1;
				pContact->contactNormal = axis;
				pContact->penetration = pen;
				pContact->one = &one;
				pContact->two = &two;
				data->contacts->Append(pContact);
				return 1;
			}
	}
	delete pContact;
	return 0;
}

// An edge on two collide with a face of one
// axis pointing from one to two
void edge2face(physicsIntegrator&one, physicsIntegrator&two,
			   vec3 axis, GLfloat pen, collisionData*data)
{
	vec3 points[2];
	for(GLint i=0;i<3;i++){
		GLfloat dot = two.getAxis(i) * axis;
		if (fabs(dot) < EPSILON) {					// Edge to face contact here
			*points[i][1] *= -1;
			continue;
		}
		if(dot > 0){
			*points[i][0] *= -1;
			*points[i][1] *= -1;
		}
	}
	
	points[0] = two.transform * points[0];
	points[1] = two.transform * points[1];
	
	axis *= (axis * (two.pi.loc - one.pi.loc) > 0)?1:-1;
	
	contact * pContact = new contact;
	pContact->penetration = pen;
	pContact->contactNormal = axis;
	pContact->one = &one;
	pContact->two = &two;
	
	pContact->contactPoint = (points[0] + points[1])/2;
	
	data->contacts->Append(pContact);
}

#define CHECK_OVERLAP(axis, index) penA[index-1]=tryAxis(one, two, axis, index, pen, best)
#define rCHECK_OVERLAP(axis, index) penA[index-1]=rtryAxis(one, two, axis, index, pen, best)
GLint collisionManager::Box2Box(physicsIntegrator&one,
								physicsIntegrator&two,
								collisionData*data) const
{	
	GLfloat penA[15];
	GLfloat pen = -BIG_VALUE;
	GLint best = 0,indexOne=1,indexTwo=1;
	
	CHECK_OVERLAP(one.getAxis(0), 1);
	CHECK_OVERLAP(one.getAxis(1), 2);
	CHECK_OVERLAP(one.getAxis(2), 3);
	
	CHECK_OVERLAP(two.getAxis(0), 4);
	CHECK_OVERLAP(two.getAxis(1), 5);
	CHECK_OVERLAP(two.getAxis(2), 6);
	
	//GLint bestAxis = best;
	
	CHECK_OVERLAP(one.getAxis(0)%two.getAxis(0), 7);
	CHECK_OVERLAP(one.getAxis(0)%two.getAxis(1), 8);
	CHECK_OVERLAP(one.getAxis(0)%two.getAxis(2), 9);
	CHECK_OVERLAP(one.getAxis(1)%two.getAxis(0), 10);
	CHECK_OVERLAP(one.getAxis(1)%two.getAxis(1), 11);
	CHECK_OVERLAP(one.getAxis(1)%two.getAxis(2), 12);
	CHECK_OVERLAP(one.getAxis(2)%two.getAxis(0), 13);
	CHECK_OVERLAP(one.getAxis(2)%two.getAxis(1), 14);
	CHECK_OVERLAP(one.getAxis(2)%two.getAxis(2), 15);
	
	
	if(pen>EPSILON*200 || pen == -BIG_VALUE)return 0;
	
	vec3 point, axis;
	point.setValue(1, 1, 1);
	vec3 toCenter = two.pi.loc - one.pi.loc;
	GLint i=0;
	
	// We've got a vertex of box two on a face of box one.
	if(best<4){
		axis = one.getAxis(best-1);
		if(axis * toCenter < 0)			// Axis pointing from one to two
			axis = -axis;
		
		for(i=0;i<3;i++){
			vec3 see = two.getAxis(i);
			GLfloat dot = two.getAxis(i) * axis;
			if(dot > 0)
			{
				*point[i] *= -1;
				indexTwo += pow(2,2-i);
			}
		}
		
		indexOne = -1;
		vec3 pt = two.transform * point;
		
		if(!Box2Point(one, pt))
		{
			indexOne = indexTwo;
			indexTwo = -1;
			point = one.transform * point;
		}else{
			point = pt;
		}
	}else if(best<7){							// We've got a vertex of box one on a face of box two.
		axis = two.getAxis(best-4);
		if(axis * toCenter < 0)					// Axis still pointing from one to two
			axis = -axis;
		
		for(i=0;i<3;i++){
			vec3 see = two.getAxis(i);
			GLfloat dot = one.getAxis(i) * axis;
			if(dot < 0)
			{
				*point[i] *= -1;
				indexOne += pow(2,2-i);
			}
		}
		
		indexTwo = -1;
		vec3 pt = one.transform * point;
		
		if(!Box2Point(two, pt))
		{
			indexTwo = indexOne;
			indexOne = -1;
			point = two.transform * point;
		}else{
			point = pt;
		}
	}else{											// Edge to Edge contact
		// Best values here got transformed into 0,1,2 respectively
		GLint ebest[2] = {(best-1)/3-2, (best-1)%3};
		axis = one.getAxis(ebest[0]).cross(two.getAxis(ebest[1]));
		if(axis.norm() < EPSILON) return false;
		
		// Axis pointing from box 1 to box 2.
		if(axis * toCenter < 0) axis = -axis;
		
		vec3 point1[2];
		point1[0].setValue(1, 1, 1);
		point1[1].setValue(1, 1, 1);
		vec3 point2[2];
		point2[0].setValue(1, 1, 1);
		point2[1].setValue(1, 1, 1);
		
		// Avoid the vertex 2 face collision index field
		indexOne += 8;
		indexTwo += 8;
		// devide the space into 3 groups of 4 lines
		indexOne += ebest[0] * 4;
		indexTwo += ebest[1] * 4;
		// Aux variable for further deviding
		GLint j = 1;
		for(i=0;i<3;i++)
		{
			if(i == ebest[1]){
				*point2[1][i] *= -1;
			}else if(two.getAxis(i) * axis > 0){
				*point2[0][i] *= -1;
				*point2[1][i] *= -1;
				indexTwo += pow(2,j);
				j--;
			}
			
			// restore the aux value
			j = 1;
			
			if(i == ebest[0]){
				*point1[1][i] *= -1;
			}else if(one.getAxis(i) * axis < 0){
				*point1[0][i] *= -1;
				*point1[1][i] *= -1;
				indexOne += pow(2,j);
				j--;
			}
		}
		
		point1[0] = one.transform * point1[0];
		point1[1] = one.transform * point1[1];
		point2[0] = two.transform * point2[0];
		point2[1] = two.transform * point2[1];
		
		vec3 a = point1[0];
		vec3 b = point1[1];
		vec3 c = point2[0];
		vec3 d = point2[1];
		vec3 p1, p2;	
		vec3 n = (b-a).cross(d-c);
		{
			vec3 n1 = b-a;
			vec3 n2 = d-c;
			GLfloat t1 = 0;
			GLfloat t2 = 0;
			
			GLfloat dem;
			if(fabs(dem = n1.x*n2.y - n1.y*n2.x) > EPSILON)
			{
				GLfloat deltaX = c.x - a.x;			// x2 - x1
				GLfloat deltaY = c.y - a.y;			// y2 - y1
				t1 = (deltaX * n2.y - deltaY * n2.x)/dem;
				t2 = (deltaX * n1.y - deltaY * n1.x)/dem;
			}else if(fabs(dem = n1.x * n2.z - n1.z * n2.x) > EPSILON){
				GLfloat deltaX = c.x - a.x;			// x2 - x1
				GLfloat deltaZ = c.z - a.z;			// y2 - y1
				t1 = (deltaX * n2.z - deltaZ * n2.x)/dem;
				t2 = (deltaX * n1.z - deltaZ * n1.x)/dem;
			}else{
				return 0;
			}
			
			p1 = a + n1 * t1;
			p2 = c + n2 * t2;
		}
		
		point = p1;
	}
	
	contact * pContact = new contact;
	pContact->SetContact(point, axis, pen, indexOne,indexTwo, &one, &two);
	data->contacts->Append(pContact);
	
	return 1;
}

// All the above codes are for coarse collision detection.
bool collisionManager::Box2Point(physicsIntegrator&target,const vec3&point) const
{
	vec3 pt = target.itransform * point;
	if(fabs(pt.x)<1 &&
	   fabs(pt.y)<1 &&
	   fabs(pt.z)<1)
		return true;
	
	return false;
}

bool collisionManager::Box2Point(physicsIntegrator&target,
								 physicsIntegrator&two,
								 const vec3&point,
								 contact*pContact) const
{
	// Transform the point into box coordinates.
	vec3 relPt = target.transform.transformInverse(point);
	
	vec3 normal;
	
	// Check each axis, looking for the axis on which the
	// penetration is least deep
	box*b = target.pi.bBox;
	GLfloat min_depth = b->halfSize.x - fabs(relPt.x);
	if(min_depth<0)return false;
	normal = target.getAxis(0) * ((relPt.x>0)?1:-1);
	
	GLfloat depth = b->halfSize.y - fabs(relPt.y);
	if(depth<0)return false;
	else if(depth<min_depth)
	{
		min_depth = depth;
		normal = target.getAxis(1) * ((relPt.y>0)?1:-1);
	}
	
	depth = b->halfSize.z - fabs(relPt.z);
	if(depth<0)return false;
	else if(depth<min_depth)
	{
		min_depth = depth;
		normal = target.getAxis(2) * ((relPt.z>0)?1:-1);
	}
	
	// Compile the contact.
	contact* c = new contact;
	c->contactNormal = normal;
	c->contactPoint = point;
	c->penetration = min_depth;
	
	// Write the appropriate data.
	c->one = &target;
	c->two = &two;
	
	pContact = c;
	return true;
}

// Assuming the normal of base plane is pointing to the point of line.
 bool collisionManager::SlabTest(const line& l, 
									   const plane& base, 
									   const GLfloat halfWidth, 
									   GLfloat& tMin, 
									   GLfloat& tMax)
{
	if((l.dir % base.n).norm() < EPSILON)return false;
	vec3 pNear = base.p + base.n * halfWidth;
	vec3 pFar = base.p - base.n * halfWidth;
	
	vec3 temp = - base.n / (l.dir * base.n);
	//tMin = MIN(-(l.p - pNear) * base.n / (l.n * base.n), tMin);
	//tMax = MAX(-(l.p - pFar) * base.n / (l.n * base.n), tMax);
	//GLfloat a = (l.p - pNear) * temp;
	//GLfloat b = (l.p - pFar) * temp;
	GLfloat nMin = (l.p - pNear) * temp;
	GLfloat nMax = (l.p - pFar) * temp;
	if(nMin > nMax)
	{
		GLfloat temp = nMin;
		nMin = nMax;
		nMax = temp;
	}
	/*if(tMin < nMin && tMax > nMax)
	 {
	 tMin = nMin;
	 tMax = nMax;
	 return true;
	 }else if(tMin > nMin && tMax < nMax)
	 return true;*/
	tMin = MAX(nMin, tMin);
	tMax = MIN(nMax, tMax);
	
	if(tMin > tMax)
		return false;
	
	return true;
}

bool collisionManager::Line2Box(const line& l, 
								const physicsIntegrator& target,
								GLfloat &min)
{
	if(l.dir.norm() < EPSILON)return false;
	GLfloat tMin = -BIG_VALUE;
	GLfloat tMax = BIG_VALUE;
	plane base;
	base.p = target.pi.loc;
	vec3 toCenter = target.pi.loc - l.p;
	vec3 halfSize = target.pi.bBox->halfSize;
	for(GLint i=0; i<3; i++)
	{
		base.n = target.getAxis(i);
		if(base.n * toCenter > 0)
			base.n *= -1;
		
		if(!SlabTest(l, base, *halfSize[i], tMin, tMax))
			return false;
	}
	//if(tMin > tMax) return false;
	if(tMin < min) min = tMin;
	
	return true;
}


